
#include "oslib.h"
#include "boxReader.h"
#include "shooter.h"
#include "claw_signal.h"
#include "rudder_chassis.h"
#include "base_chassis.h"
#include "../race_tasks/race_tasks.h"
#include "laser.h"

extern osMessageQueueId_t CtrlBtnQueueHandle;

void button_exe(int group, int btn, int press)
{
    // uprintf("Group=%d, Button=%d, Press=%d\n", group, btn, press);
    int id = group * 10 + btn;
    if (press)
    { // 按下
        switch (id)
        {
        case 70:
            // aim
            shooter_start_aim(shooter1);
            break;
        case 71:
            // manualAiming
            changeManul();
            break;
        case 72:
            // autoAiming
            changeAuto();
            break;
        case 73:
            // shoot
            shooter_test_shoot(shooter1);
            break;
        case 76:
            manualArrive();
            break;
        case 77:
            manualRightStart();
            break;
        case 78:
            manualLeftStart();
            break;
        case 79:
            Signal_StartTakeArrow();
            break;
        default:
            break;
        }
    }
    else
    { // 释放
        switch (id)
        {
        case 0:
            uprintf("--Handle|Switch print handle status!\r\n");
            SW_Handle_PrintRockerStatus_Flag = (SW_Handle_PrintRockerStatus_Flag + 1) % 2;
            break;
        case 1:
            // SW_InitCalibration();
            break;
        case 2: // 串口显示定位信息
            Chassis_PrintPostureStatus_Flag = (Chassis_PrintPostureStatus_Flag + 1) % 2;
            uprintf("--Print chassis status change to %d\r\n", Chassis_PrintPostureStatus_Flag);
            break;
        case 3: // 切换坐标模式
            uprintf("--Chassis pos mode changed\r\n");
            BaseChassis.pos_mode = (BaseChassis.pos_mode + 1) % 2;
            switch (BaseChassis.pos_mode)
            {
            case POS_MODE_ABSOLUTE:
                uprintf("--Pos mode change to absolute\r\n");
                break;
            case POS_MODE_RELATIVE:
                uprintf("--Pos mode change to relative\r\n");
                break;
            }
            break;
        case 4: // 继电器通电
            uprintf("\r\n## Realay has been ACCESSED! Please be careful! ##\r\n");
            HAL_GPIO_WritePin(Relay_GPIO_Port, Relay_Pin, RELAY_ACCESS);
            break;
        case 5: // 紧急停止，继电器断电
            uprintf("\r\n## Emergency STOP! Realay has been BROKEN! ##\r\n");
            HAL_GPIO_WritePin(Relay_GPIO_Port, Relay_Pin, RELAY_BROKEN);
            break;
        case 6:                                                      // 切换控制模式
            // BaseChassis.ctrl_mode = (BaseChassis.ctrl_mode + 1) % 4; // 不进入调试模式
            BaseChassis.ctrl_mode = CTRL_MODE_HANDLE;
            BaseChassis.handbrake_flag = 0;
            switch (BaseChassis.ctrl_mode)
            {
            case 0:
                break;
            case 1:
                uprintf("Handle|Ctrl mode change to HANDLE\r\n");
                BaseChassis.pos_mode = POS_MODE_RELATIVE;
                break;
            case 2:
                uprintf("Handle|Ctrl mode change to CMD\r\n");
                break;
            case 3:
                uprintf("Handle|Ctrl mode change to AUTO\r\n");
                BaseChassis.pos_mode = POS_MODE_ABSOLUTE;
                break;
            }
            break;
        case 7: // 开启偏航角锁止模式
            BaseChassis.lock_yaw_flag = (BaseChassis.lock_yaw_flag + 1) % 2;
            BaseChassis.target_yaw = BaseChassis.PostureStatus.yaw;
            if (BaseChassis.lock_yaw_flag)
            {
                uprintf("Handle|lock yaw started. target_yaw:%5.2f\r\n", BaseChassis.PostureStatus.yaw);
            }
            else
            {
                uprintf("Handle|lock yaw closed\r\n");
            }
            break;
        case 8:
            RaceStateMachine.state = RS_AT_HOME;
            uprintf("Handle|RSM change to AT_HOME!\r\n");
            break;
        case 9:
            float laser_dist = laser_get_dist(chassis_laser1);
            uprintf("laser_dist:%.5f\r\n", laser_dist);
            break;

        /*10系指令用于底盘轨迹跟踪控制*/
        case 10: // 舵轮零位校正
            // SW_InitCalibration();
            break;
        case 11: // 跳出校正循环
            SW_InitCalibration_BreakFlag = 1;
            uprintf("--Handle: Jump out of calibraiton loop!\r\n");
            break;
        case 12:
            BaseChassis.TrackStatus.track_path_index = 0;
            Chassis_TrackStatus_Reset();
            uprintf("--Handle: Track status has been reseted! \r\n");
            break;
        case 13:
            Chassis_Go2Point_Reset();
            uprintf("--Handle: go2point status has been reseted! \r\n");
            break;
        case 14: // 继电器通电
            uprintf("\r\n## Realay has been ACCESSED! Please be careful! ##\r\n");
            HAL_GPIO_WritePin(Relay_GPIO_Port, Relay_Pin, RELAY_ACCESS);
            break;
        case 15: // 紧急停止，继电器断电
            uprintf("\r\n## Emergency STOP! Realay has been BROKEN! ##\r\n");
            HAL_GPIO_WritePin(Relay_GPIO_Port, Relay_Pin, RELAY_BROKEN);
            BaseChassis.ctrl_mode = CTRL_MODE_NONE;
            break;
        case 16:
            Chassis_TrackStatus_Reset();
            BaseChassis.ctrl_mode = CTRL_MODE_TRACK;
            BaseChassis.TrackStatus.track_path_index = 2;
            BaseChassis.TrackStatus.start = 1;
            uprintf("--Handle: TrackSets has been reseted! Start tracking path1.\r\n");
            break;
        case 17:
            Chassis_Go2Point_Reset();
            BaseChassis.ctrl_mode = CTRL_MODE_GO_TO_POINT;
            BaseChassis.Go2PointStatus.target_point.x = ArrowFramePoint2.x;
            BaseChassis.Go2PointStatus.target_point.y = ArrowFramePoint2.y;
            BaseChassis.Go2PointStatus.target_yaw = 1.5708;
            BaseChassis.Go2PointStatus.start_speed = 0.35;
            BaseChassis.Go2PointStatus.final_speed = 0;
            BaseChassis.Go2PointStatus.start = 1;
            BaseChassis.Go2PointStatus.enable = 1;
            BaseChassis.Go2PointStatus.disable_yaw_ctrl = 1;
            uprintf("--Handle|Go to arrow frame point2...");
            break;
        case 18:
            Chassis_Go2Point_Reset();
            BaseChassis.Go2PointStatus.disable_yaw_ctrl = 0;
            BaseChassis.ctrl_mode = CTRL_MODE_GO_TO_POINT;
            BaseChassis.Go2PointStatus.target_point.x = ShootPoint1.x;
            BaseChassis.Go2PointStatus.target_point.y = ShootPoint1.y;
            BaseChassis.Go2PointStatus.target_yaw = 0.785;
            BaseChassis.Go2PointStatus.start_speed = 0.3;
            BaseChassis.Go2PointStatus.final_speed = 0;
            BaseChassis.Go2PointStatus.start = 1;
            BaseChassis.Go2PointStatus.enable = 1;
            uprintf("--Handle|Go to shoot point1...");
            break;
        case 19:
            Chassis_Go2Point_Reset();
            BaseChassis.Go2PointStatus.disable_yaw_ctrl = 0;
            BaseChassis.ctrl_mode = CTRL_MODE_GO_TO_POINT;
            BaseChassis.Go2PointStatus.target_point.x = ShootPoint2.x;
            BaseChassis.Go2PointStatus.target_point.y = ShootPoint2.y;
            BaseChassis.Go2PointStatus.target_yaw = 0.22;
            BaseChassis.Go2PointStatus.start_speed = 0.3;
            BaseChassis.Go2PointStatus.final_speed = 0;
            BaseChassis.Go2PointStatus.start = 1;
            BaseChassis.Go2PointStatus.enable = 1;
            uprintf("--Handle|Go to shoot point2...");
            break;
            break;

        case 77:
            manualRightStop();
            break;
        case 78:
            manualLeftStop();
            break;
        default:
            break;
        }
    }
}

void ctrl_btn_task(void *argument)
{
    static CAN_ConnMessage msg;
    static uint16_t btn_last_value = 0;
    static uint16_t btn_last_group = 0;
    static uint16_t btn_pending = 0;
    for (;;)
    {
        osMessageQueueGet(CtrlBtnQueueHandle, &msg, NULL, osWaitForever);
        uint16_t btn_value = msg.payload.ui16[0];
        uint16_t btn_group = msg.payload.ui16[1];
        uint16_t btn_changed = btn_value ^ btn_last_value;
        if (btn_group != btn_last_group)
        { // 分组发生改变
            if (btn_last_value != 0)
            {
                // 旧按键直接释放
                for (int i = 0; i < 10; i++)
                {
                    if ((1UL << i) & btn_last_value)
                    {
                        if ((1UL << i) & btn_pending)
                        {
                            btn_pending &= ~(1UL << i); // 释放抵消
                            continue;
                        }
                        button_exe(btn_last_group, i, 0);
                    }
                }
            }
            if (btn_value != 0)
            {
                // 新按键(不允许)直接按下
                for (int i = 0; i < 10; i++)
                {
                    if ((1UL << i) & btn_value)
                    {
                        btn_pending |= (1UL << i); // 下次释放会被抵消
                    }
                }
            }
        }
        else
        { // 分组未发生改变
            // 检查发生变化的位, 依次进行按下/释放
            if (btn_changed != 0)
            {
                for (int i = 0; i < 10; i++)
                {
                    if ((1UL << i) & btn_changed)
                    {
                        uint8_t press = ((1UL << i) & btn_value) ? 1 : 0;
                        if (press == 0 && ((1UL << i) & btn_pending))
                        {
                            btn_pending &= ~(1UL << i); // 释放抵消
                            continue;
                        }
                        button_exe(btn_group, i, press);
                    }
                }
            }
        }
        btn_last_group = btn_group;
        btn_last_value = btn_value;
    }
}